/*
**********************************************************************************
    OpenSURF642 : An TI DM642 implementation of OpenSURF.
    Copyright (C) 2011  Hai Yu

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
    
	This implementation is based on The original OpenSURF library created
	by Christopher Evans.The original work can be found at
		http://www.chrisevansdev.com/computer-vision-opensurf.html
	
    contact: yuhaihai12345@gmail.com	
**********************************************************************************
*/

#include "fasthessian.h"
#include "MatrixInverse.h"
#include "MatrixMultiply.h"
#include <assert.h>
#include "fastrts62x64x.h"

FastHessians * CreateFastHessians(	IntegralImage * image, 
									Uint16 Octaves,
									Uint16 Intervals,
									Uint16 init_samples, 
									ResponseDataType thres)
{
	int i;
	
	FastHessians * fh = (FastHessians *)malloc(sizeof(FastHessians));
	
	fh->ipts = (IPoint *) malloc(MAX_IPTS * sizeof(IPoint));
	fh->ipts_count = 0;

	fh->responseMap_count = 0;
	for(i = 0; i < MAX_RESPONSELAYER; i++)
	{
		fh->responseMap[i] = 0;
	}

	SetImage(fh, image);

	SetParameter(fh, Octaves, Intervals, init_samples, thres);
	return fh;

}

void DestroyFastHessians(FastHessians * self)
{
	int i;
	for(i = 0; i < self->responseMap_count; ++i)
		DeleteResponseLayer(self->responseMap[i]);

	free(self);
}

void SetImage(FastHessians * self, IntegralImage * image)
{
	self->image = image;
	self->i_height = image->lines;
	self->i_width  = image->pixels_per_line;
}

void SetParameter(	FastHessians * self, 
                    Uint16 octaves, 
                    Uint16 intervals, 
                    Uint16 init_samples, 
                    ResponseDataType thres)
{
    self->octaves = octaves;
    self->intervals = intervals;
    self->init_samples = init_samples;
    self->thres = thres;
}

//! Build map of DoH responses
void buildResponseMap(FastHessians * self)
{
	// Calculate responses for the first 4 octaves:
	// Oct1: 9,  15, 21, 27
	// Oct2: 15, 27, 39, 51
	// Oct3: 27, 51, 75, 99
	// Oct4: 51, 99, 147,195
	// Oct5: 99, 195,291,387
	
	
	// Get image attributes
  	int w = (self->i_width / self->init_samples);
  	int h = (self->i_height / self->init_samples);
  	int s = (self->init_samples);
	unsigned int i = 0;
	// Deallocate memory and clear any existing response layers
	for(i = 0; i < self->responseMap_count; ++i)  
		DeleteResponseLayer(self->responseMap[i]);
  	
	self->responseMap_count = 0;

	
  	// Calculate approximated determinant of hessian values
  	if (self->octaves >= 1)
  	{
		self->responseMap[0] = CreateResponseLayer(w,h,s,9);
		self->responseMap[1] = CreateResponseLayer(w,h,s,15);
		self->responseMap[2] = CreateResponseLayer(w,h,s,21);
		self->responseMap[3] = CreateResponseLayer(w,h,s,27);
		self->responseMap_count = 4;
  	}
 
  	if (self->octaves >= 2)
  	{
		self->responseMap[4] = CreateResponseLayer(w/2, h/2, s*2, 39);
		self->responseMap[5] = CreateResponseLayer(w/2, h/2, s*2, 51);
		self->responseMap_count = 6;
  	}

  	if (self->octaves >= 3)
  	{
		self->responseMap[6] = CreateResponseLayer(w/4, h/4, s*4, 75);
		self->responseMap[7] = CreateResponseLayer(w/4, h/4, s*4, 99);
		self->responseMap_count = 8; 
  	}

  	if (self->octaves >= 4)
  	{
		self->responseMap[8] = CreateResponseLayer(w/8, h/8, s*8, 147);
		self->responseMap[9] = CreateResponseLayer(w/8, h/8, s*8, 195);
		self->responseMap_count = 10;
  	}

  	if (self->octaves >= 5)
  	{
		self->responseMap[10] = CreateResponseLayer(w/16, h/16, s*16, 291);
		self->responseMap[11] = CreateResponseLayer(w/16, h/16, s*16, 387);
		self->responseMap_count = 12;
  	}
	
  	// Extract responses from the image
  	for (i = 0; i < self->responseMap_count; ++i)
  	{
    	buildResponseLayer(self, self->responseMap[i]);
  	}
	
	
}

void buildResponseLayer(FastHessians * restrict self, ResponseLayer * restrict rl)
{
	//ResponseDataType Dxx, Dyy, Dxy;
    int Dxx, Dyy, Dxy;
    int r,c,ar = 0;
    int ac = 0;
    int index = 0;
    ResponseDataType *responses = rl->responses;         // response storage
    unsigned char *laplacian = rl->laplacians; // laplacian sign storage
    int step = rl->step;                      // step size for this filter
    int b = (rl->filter - 1) / 2 + 1;         // border for this filter
    int l = rl->filter / 3;                   // lobe for this filter (filter size / 3)
    int w = rl->filter;                       // filter size
    
   	int area = w*w;
  	int area_sq = (area * area * 2);
  	for(r=0, c=0, ar = 0, index = 0; ar < rl->height; ++ar) 
  	{
		r = ar * step;
    	for(ac = 0; ac < rl->width; ++ac, index++) 
    	{
      		// get the image coordinates
            
            c = ac * step; 

            // Compute response components
            Dxx = BoxIntegral(self->image, r - l + 1, c - b, 2*l - 1, w)
                - BoxIntegral(self->image, r - l + 1, c - l / 2, 2*l - 1, l)*3;
            Dyy = BoxIntegral(self->image, r - b, c - l + 1, w, 2*l - 1)
                - BoxIntegral(self->image, r - l / 2, c - l + 1, l, 2*l - 1)*3;
            Dxy =   BoxIntegral(self->image, r - l, c + 1, l, l)
                  + BoxIntegral(self->image, r + 1, c - l, l, l)
                  - BoxIntegral(self->image, r - l, c - l, l, l)
                  - BoxIntegral(self->image, r + 1, c + 1, l, l);
            
			Dxy = _sshvr(Dxy*461,9);
			
            // Get the determinant of hessian response & laplacian sign
			responses[index] = _sshvr((Dxx/area) * (Dyy/area),1) - Dxy * Dxy / area_sq; 
            laplacian[index] = (Dxx + Dyy >= 0 ? 1 : 0);
      }
  }
}

void DetectIPoints( FastHessians * self)
{
	
  	// filter index map
	static const int filter_map [OCTAVES][INTERVALS] = {{0,1,2,3}, {1,3,4,5}, {3,5,6,7}, {5,7,8,9}, {7,9,10,11}};

	int i, r, c, o;

	ResponseLayer *b, *m, *t;
  	// Clear the vector of exisiting ipts
	self->ipts_count = 0;

  	// Build the response map
  	buildResponseMap(self);

 	// Get the response layers
  	
  	for (o = 0; o < self->octaves; ++o) 
  		for (i = 0; i <= 1; ++i)
  		{
    		b = self->responseMap[filter_map[o][i]];
    		m = self->responseMap[filter_map[o][i+1]];
    		t = self->responseMap[filter_map[o][i+2]];

    		// loop over middle response layer at density of the most 
    		// sparse layer (always top), to find maxima across scale and space
    		for (r = 0; r < t->height; ++r)
    		{
      			for (c = 0; c < t->width; ++c)
      			{
        			if (isExtremum(self,r, c, t, m, b))
        			{
          				interpolateExtremum(self,r, c, t, m, b);
        			}
      			}
    		}
  		}	 

}

//! Interpolate scale-space extrema to subpixel accuracy to form an image feature.  
void interpolateExtremum(	FastHessians * self, 
                            Uint16 r,
                            Uint16 c,
                            ResponseLayer * t,
                            ResponseLayer * m,
                            ResponseLayer * b)
{

    float xi = 0, xr = 0, xc = 0;
    // get the step distance between filters
    // check the middle filter is mid way between top and bottom
    int filterStep = (m->filter - b->filter);
    assert((filterStep > 0) && ((t->filter - m->filter) == (m->filter - b->filter)));

    interpolateStep(self,r, c, t, m, b, &xi, &xr, &xc );
    if( fabs( xi ) < 0.5f  &&  fabs( xr ) < 0.5f  &&  fabs( xc ) < 0.5f )
  	{
        if(self->ipts_count >= MAX_IPTS)
            return;
        self->ipts_count++;
        self->ipts[self->ipts_count-1].x = (float)((c + xc) * t->step);
        self->ipts[self->ipts_count-1].y = (float)((r + xr) * t->step);
        self->ipts[self->ipts_count-1].scale = (float)((0.1333f) * (m->filter + xi * filterStep));
        self->ipts[self->ipts_count-1].laplacian = (GetLaplacian2(m,r,c,t));
       
  	}
}


void interpolateStep(	FastHessians * self,
                        Uint16 r,
                        Uint16 c,
                        ResponseLayer * t,
                        ResponseLayer * m,
                        ResponseLayer * b,
                        float * xi,
                        float * xr,
                        float * xc)
{
    Matrix * dD, * H, * H_inv, *X;
    

    dD = deriv3D( r, c, t, m, b );
    H = hessian3D( r, c, t, m, b );

    H_inv = CreateMatrix(3,3);
    MatrixInverseFloat((float *)(H->data), (float *)(H_inv->data),3,3);

    X = CreateMatrix(3,1);  
    MatrixMultiplyFloat(H_inv->data, 3, 3, dD->data, 3,1,X->data);

    DeleteMatrix(dD);
    DeleteMatrix(H);
    DeleteMatrix(H_inv);	

    *xi = -X->data[2];
    *xr = -X->data[1];
    *xc = -X->data[0];

    DeleteMatrix(X);
}

Uint8 isExtremum(	FastHessians * self,
                    Uint16 r,
                    Uint16 c,
                    ResponseLayer * t,
                    ResponseLayer * m,
                    ResponseLayer * b)
{

    int rr,cc;
    // bounds check
    int layerBorder = (t->filter + 1) / (2 * t->step);
    ResponseDataType candidate = GetResponse2(m,r, c, t);
    if (r <= layerBorder || r >= t->height - layerBorder || c <= layerBorder || c >= t->width - layerBorder)
        return 0;

    // check the candidate point in the middle layer is above thresh 
      
    if (candidate < self->thres) 
        return 0; 

	#pragma MUST_ITERATE (3,3);
    for (rr = -1; rr <=1; ++rr)
    {
		#pragma MUST_ITERATE (3,3);
        for (cc = -1; cc <=1; ++cc)
        {
          	// if any response in 3x3x3 is greater candidate not maximum
          	if (
            	GetResponse(t,r+rr, c+cc) >= candidate ||
            	((rr != 0 || cc != 0) && GetResponse2(m,r+rr, c+cc, t) >= candidate) ||
            	GetResponse2(b,r+rr, c+cc, t) >= candidate
            ) 
            	return 0;
        }
      }

      return 1;

}


Matrix * hessian3D(	Uint16 r,
                    Uint16 c,
                    ResponseLayer * t,
                    ResponseLayer * m,
                    ResponseLayer * b)
{
    Matrix * matrix = CreateMatrix(3,3);
    ResponseDataType v, dxx, dyy, dss, dxy, dxs, dys;
    
    v 	= GetResponse2(m,r, c, t);
    dxx = GetResponse2(m,r, c + 1, t) + GetResponse2(m,r, c - 1, t) - 2 * v;
    dyy = GetResponse2(m, r + 1, c, t) + GetResponse2(m, r - 1, c, t) - 2 * v;
    dss = GetResponse(t, r, c) + GetResponse2(b, r, c, t) - 2 * v;
    dxy = ( GetResponse2(m, r + 1, c + 1, t) - GetResponse2(m, r + 1, c - 1, t) - 
              GetResponse2(m, r - 1, c + 1, t) + GetResponse2(m, r - 1, c - 1, t) ) >> 2;
    dxs = ( GetResponse(t, r, c + 1) - GetResponse(t, r, c - 1) - 
              GetResponse2(b, r, c + 1, t) + GetResponse2(b, r, c - 1, t) ) >> 2;
    dys = ( GetResponse(t, r + 1, c) - GetResponse(t, r - 1, c) - 
              GetResponse2(b, r + 1, c, t) + GetResponse2(b, r - 1, c, t) ) >> 2;

    matrix->data[0] = divsp(intsp(dxx),32767.0f);
    matrix->data[1] = divsp(intsp(dxy),32767.0f);
    matrix->data[2] = divsp(intsp(dxs),32767.0f);

    matrix->data[3] = divsp(intsp(dxy),32767.0f);
    matrix->data[4] = divsp(intsp(dyy),32767.0f);
    matrix->data[5] = divsp(intsp(dys),32767.0f);

    matrix->data[6] = divsp(intsp(dxs),32767.0f);
    matrix->data[7] = divsp(intsp(dys),32767.0f);
    matrix->data[8] = divsp(intsp(dss),32767.0f);
    return matrix;
}

Matrix * deriv3D(	Uint16 r,
                    Uint16 c,
                    ResponseLayer * t,
                    ResponseLayer * m,
                    ResponseLayer * b)
{
    Matrix * matrix = CreateMatrix(3,1);
    ResponseDataType dx, dy, ds;

    dx = (GetResponse2(m, r, c+1, t) - GetResponse2(m, r, c-1, t)) >> 1;
    dy = (GetResponse2(m, r+1, c, t) - GetResponse2(m, r-1, c, t)) >> 1;
    ds = (GetResponse(t,r, c ) - GetResponse2(b, r, c, t)) >>1;

    matrix->data[0] = divsp(intsp(dx),32767.0f);
    matrix->data[1] = divsp(intsp(dy),32767.0f);
    matrix->data[2] = divsp(intsp(ds),32767.0f);

   	return matrix;

}
